U = zeros(1,5000);
Y = zeros(1,5000);
delt_T = 0.001;
K=2;
Kd=1;
Ki=0.5;
uk_1 = 0;
uk = 0;
u = 0;
y2k_1=0;
for i=1:1: 5000
    if (i==1000)
        u=1;
    end
    uk_1 = uk;
    uk = u;
    y1k=K*uk+Kd*((uk-uk_1)/delt_T);
    y2k =y2k_1+delt_T*Ki*u;
    Y(i)=y1k+y2k;
    U(i) =u;
    y2k_1=y2k;
end
t = 0: 0.001 : 4.999;
hold on;
plot(t,Y)
hold on;
plot(t,U)
